#include <ros/ros.h>
#include <hellobot_msgs/msg_call.h>
#include <hellobot_msgs/msg_pose.h>
#include <hellobot_msgs/msg_sim.h>
#include <ros/package.h>
#include <unistd.h>
#include <time.h>
#include <iostream>

using namespace std;
extern float bot_pose[9]={0};//the pose of the robot
extern float target[3]={0};
extern bool IsNewTarget = false;

hellobot_msgs::msg_call Aim(char flag)
{   
    //cout << flag << endl;
    hellobot_msgs::msg_call msg;
    if (flag == '1')
    {
        //hellobot_msgs::msg_call msg;
        msg.newstatus = true;
        msg.x = bot_pose[0]-1;
        msg.y = bot_pose[1]-5;
        msg.z = bot_pose[2];
        cout << flag << endl;
    }else if (flag == '2')
    {
        //hellobot_msgs::msg_call msg;
        msg.newstatus = true;
        msg.x = bot_pose[0];
        msg.y = bot_pose[1];
        msg.z = bot_pose[2];
    }else if (flag == '3')
    {
        //hellobot_msgs::msg_call msg;
        msg.newstatus = false;
        msg.x = 0;
        msg.y = 0;
        msg.z = 0;
    }

    return msg;
}

void IMUCallback(const hellobot_msgs::msg_poseConstPtr &msg)
{
    //refresh the bot_pose 
    bot_pose[0] = msg->x;
    bot_pose[1] = msg->y;
    bot_pose[2] = msg->z;
    bot_pose[3] = msg->alpha;
    bot_pose[4] = msg->beta;
    bot_pose[5] = msg->gamma;
}

void SIMCallback(const hellobot_msgs::msg_simConstPtr &msg)
{
    //refresh the bot_pose 
    bot_pose[0] = msg->rx;
    bot_pose[1] = msg->ry;
    bot_pose[2] = msg->rz;
    bot_pose[3] = msg->ralpha;
    bot_pose[4] = msg->rbeta;
    bot_pose[5] = msg->rgamma;
    target[0] = msg->tx;
    target[1] = msg->ty;
    target[2] = msg->tz;
    IsNewTarget = msg->newstatus;
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"MicroPhoneArray");
    ros::NodeHandle nh;

    ros::Subscriber imu_sub=nh.subscribe<hellobot_msgs::msg_pose>("Pose",1,IMUCallback);
    ros::Subscriber sim_sub=nh.subscribe<hellobot_msgs::msg_sim>("SIM",10,SIMCallback);
    ros::Publisher calling_pub=nh.advertise<hellobot_msgs::msg_call>("Calling",10);

    ros::Duration(2).sleep();
    ros::Rate rate(60); 

    hellobot_msgs::msg_call msg;
    //time_t time_begin, time_end;
    //bool time_flag = true;
    //double time_used = 0;
    //char command = '3';
    //time_begin = time(NULL);
    while (ros::ok())
    {
        {
            // time simulation
            /* time_end = time(NULL);;
            time_used = double(difftime(time_end, time_begin));
            //cout << time_used << endl;
            //cout << time_flag << endl;
            if (time_flag == true)
            {
                if (time_used > 5)
                {
                    time_flag = false;
                    msg = Aim('1');
                    calling_pub.publish(msg);
                    time_begin = time(NULL);;
                }else{
                    msg = Aim('3');
                    calling_pub.publish(msg);
                }                  
            }else{
                if (time_used > 15)
                {
                    time_flag = true;
                    msg = Aim('2');
                    calling_pub.publish(msg);
                    time_begin = time(NULL);;
                }else{
                    msg = Aim('3');
                    calling_pub.publish(msg);
                }  
            } */

            // Pybullet simulation
            msg.newstatus = IsNewTarget;
            if (IsNewTarget == true)
            {
                msg.x = target[0];
                msg.y = target[1];
                msg.z = target[2];
                msg.yaw = bot_pose[5];
            }/* else{
                msg.x = target[0]-bot_pose[0];
                msg.y = target[1]-bot_pose[1];
                msg.z = target[2]-bot_pose[2];
            } */
            calling_pub.publish(msg);

            //msg = Aim('1');
            //calling_pub.publish(msg);
            //msg = Aim();
            //calling_pub.publish(msg);
            /* sleep(5);
            msg.newstatus = false;
            calling_pub.publish(msg); */
            //ROS_INFO(msg.newstatus);

        }
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}   